#include "usr_inc.h"

/* 传球参数 */
float s = 0.01;        // 出射点到底盘中心的水平距离, 单位为m
float w = 0.9;         // 出射点到底盘中心的竖直距离, 单位为m
float H = HOOP_HEIGHT; // 篮筐的高度
float h = R2_HEIGHT;   // R2接球点的高度
float k = 1.9626;      // 出射斜率, k=tan63°

/**
 * @brief 获取投篮的推程
 * @param distance: 篮筐到底盘中心的水平距离, 单位为m
 * @return 推程
 */
float TOSS_GetHoopStroke(float distance)
{
    return 0.045964 * distance + 0.170037;
}

/**
 * @brief 求传球解析解的函数
 */
float func(float D, void *ctx)
{
    float d = *(float *)ctx;

    float a = (H - w) / powf(D - s, 2) - k / (D - s);
    float b = k - 2 * s * a;
    float c = w - powf(s, 2) * a - s * b;

    return a * powf(d, 2) + b * d + c - h;
}

/**
 * @brief 求解传球解析解的函数导数
 */
float dfunc(float D, void *ctx)
{
    float d = *(float *)ctx;

    float da = -2 * (H - w) / powf(D - s, 3) + k / powf(D - s, 2);
    float db = -2 * s * da;
    float dc = -powf(s, 2) * da - s * db;

    return da * powf(d, 2) + db * d + dc;
}

void StartTossCmdDeal(void *argument)
{
    for (;;)
    {
        /* 推射指令 */
        TossCmdMsg_t cmd;
        if (xQueueReceive(tossCmdQueueHandle, &cmd, 1) == pdTRUE)
        {
            if (cmd == TOSS_DRAG)
            {
                BSP_FDCAN_SendData(&hfdcan1, 0x201, NULL, FDCAN_DLC_BYTES_0);
            }
            else if (cmd == TOSS_FREE)
            {
                BSP_FDCAN_SendData(&hfdcan1, 0x203, NULL, FDCAN_DLC_BYTES_0);
                BSP_FDCAN_SendData(&hfdcan2, 0x161, NULL, FDCAN_DLC_BYTES_0);
                vTaskDelay(pdMS_TO_TICKS(1200));
                BSP_FDCAN_SendData(&hfdcan2, 0x160, NULL, FDCAN_DLC_BYTES_0);
            }
            else if (cmd == TOSS_ABANDON)
            {
                BSP_FDCAN_SendData(&hfdcan1, 0x204, NULL, FDCAN_DLC_BYTES_0);
            }
            else if (cmd == TOSS_UP)
            {
                BSP_FDCAN_SendData(&hfdcan1, 0x211, NULL, FDCAN_DLC_BYTES_0);
            }
            else if (cmd == TOSS_DOWN)
            {
                BSP_FDCAN_SendData(&hfdcan1, 0x212, NULL, FDCAN_DLC_BYTES_0);
            }
        }

        /* 推程 */
        float stroke;
        if (xQueueReceive(tossStrokeQueueHandle, &stroke, 1) == pdTRUE)
        {
            uint8_t buf[4];
            memcpy(buf + 0, &stroke, sizeof(float));
            BSP_FDCAN_SendData(&hfdcan1, 0x220, buf, FDCAN_DLC_BYTES_4);
        }

        /* 射球参数 */
        ShootArgsMsg_t args;
        if (xQueueReceive(shootArgsQueueHandle, &args, 1) == pdTRUE)
        {
            /* 投篮 */
            if (fabsf(args.h - 2.43) <= 1e-5)
            {
                float str = TOSS_GetHoopStroke(args.d);

                uint8_t buf[4];
                memcpy(buf + 0, &str, sizeof(float));
                BSP_FDCAN_SendData(&hfdcan1, 0x220, buf, FDCAN_DLC_BYTES_4);
            }
            /* 传球 */
            else if (fabsf(args.h - R2_HEIGHT) <= 1e-5)
            {
                float res;
                NewtonStatus_t sta = ALGO_NewtonMethod(func, dfunc, args.d, &(args.d), &res);

                if (sta == NEWTON_SUCCESS)
                {
                    float str = TOSS_GetHoopStroke(res);

                    uint8_t buf[4];
                    memcpy(buf + 0, &str, sizeof(float));
                    BSP_FDCAN_SendData(&hfdcan1, 0x220, buf, FDCAN_DLC_BYTES_4);
                }
            }
        }
    }
}
